#ifndef ENCODER_HPP
#define ENCODER_HPP

namespace 
{
  class InterruptHandler
{
  public:
  virtual void onInterrupt()=0;
};
InterruptHandler* IntHandlers[2];
void int0Func()
{
  IntHandlers[0]->onInterrupt();
}
void int1Func()
{
  IntHandlers[1]->onInterrupt();
}

void(*intHandlerFunctions[])() = {int0Func, int1Func};
//Deregistration is not possible for now
void RegisterForInterrupt(unsigned int interruptNumber, InterruptHandler* handlerObject)
{
    IntHandlers[interruptNumber] = handlerObject;
    attachInterrupt(interruptNumber, intHandlerFunctions[interruptNumber], CHANGE);         // configure interrupt for the BMA on rising edge

}
}


class Encoder: public InterruptHandler
{
public:
    Encoder(short encoderPin)
    {
        pinMode(encoderPin, INPUT);
        RegisterForInterrupt(encoderPin-2,this);

    }
    void periodicMeasurementCheck()
    {
        if(digitalRead(_encoderPin)==HIGH)
        {
            measurementUpdate(_measuredTime, _timestamp);
            periodicSpeedUpdate();
        }
        _encoder = false;

    }
    void periodicMeasurementCheck()
    {
        if(digitalRead(_encoderPin)==HIGH)
        {
            measurementUpdate(_measuredTime, _timestamp);
            periodicSpeedUpdate();
        }
        _encoder = false;

    }
    void onInterrupt() //bedzie potezny blad raz na ~70 minut, ale nie ma potrzeby sie nim przejmowac
    {
        measurementUpdate(_measuredTime, _timestamp);
    }
    void measurementUpdate(unsigned long& currTime, unsigned long& timestamp)
    {
      unsigned long time = micros();
      currTime = time - timestamp;
      timestamp = time;
    }

private:
    bool _encoder;
    short _encoderPin;
    unsigned long _measuredTime;
    unsigned long _timestamp;

};
#endif // ENCODER_HPP
